pi = 3.1415926;
% x = linspace(0, 1, 100);  % 方向盘转角归一化到[0,1]
% x = [0.65 0.2 0.03 0.1 0.02];
 x = [1 0 0 0 0];
% x = [0.95 0.05 0.00 0.0 0.00];
% x = [0.05 0.95 0.00 0.0 0.00];
% x = [0.05 0.05 0.00 0.9 0.00];

%SU 
train_data_SU =1 - 0.4*linspace(0, 1, 20);
c = mean(train_data_SU);
sigma = std(train_data_SU);
rho =  Margin_t(x)*(1 - normalized_entropy(x));

% Initialize the command and its membership
SU = FuzzyCommand('SU', c, sigma, rho);
SU.CurrentProb = x(1);
SU = SU.compute_membership(x(1));
SU = SU.record_history();

SD = FuzzyCommand('SD', c, sigma, rho);
SD.CurrentProb = x(2);
SD = SD.compute_membership(SD.CurrentProb);
SD = SD.record_history();

TL = FuzzyCommand('TL', c, sigma, rho);
TL.CurrentProb = x(3);
TL = TL.compute_membership(TL.CurrentProb);
TL = TL.record_history();

TR = FuzzyCommand('TR', c, sigma, rho);
TR.CurrentProb = x(4);
TR = TR.compute_membership(TR.CurrentProb);
TR = TR.record_history();

NC = FuzzyCommand('NC', c, sigma, rho);
NC.CurrentProb = x(5);
NC = NC.compute_membership(NC.CurrentProb);
NC = NC.record_history();

% disp(SU.HMembership);
% disp(SU.LMembership);
% disp(SD.HMembership);
% disp(SD.LMembership);
% 
% disp(TL.HMembership);
% disp(TR.HMembership);
% disp(NC.HMembership);

% Get the second-order membership
Linear_SU_rule = FuzzyRules('Linear_SU_rule',SU.HMembership, SD.LMembership, SU.CurrentProb, ...
    SD.CurrentProb, 0.6, -0.6, 0.01);
Linear_NC1_rule = FuzzyRules('Linear_NC1_rule',SU.HMembership, SD.HMembership, SU.CurrentProb, ...
    SD.CurrentProb, 0.3, -0.3, 0.01);
Linear_SD_rule = FuzzyRules('Linear_SD_rule',SU.LMembership, SD.HMembership, SU.CurrentProb, ...
    SD.CurrentProb, 0.6, -0.6, 0.0001);
Linear_NC2_rule = FuzzyRules('Linear_NC2_rule',SU.LMembership, SD.LMembership, SU.CurrentProb, ...
    SD.CurrentProb, 0.3, -0.3, 0.0001);

Angular_TL_rule = FuzzyRules('Angular_TL_rule',TL.HMembership, TR.LMembership, TL.CurrentProb, ...
    TR.CurrentProb, pi/6, -pi/6, 0.01);
Angular_NC1_rule = FuzzyRules('Angular_NC1_rule',TL.HMembership, TR.HMembership, TL.CurrentProb, ...
    TR.CurrentProb, pi/24, -pi/24, 0.01);
Angular_TR_rule = FuzzyRules('Angular_TR_rule',TL.LMembership, TR.HMembership, TL.CurrentProb, ...
    TR.CurrentProb, pi/6, -pi/6, 0.0001);
Angular_NC2_rule = FuzzyRules('Angular_NC2_rule',TL.LMembership, TR.LMembership, TL.CurrentProb, ...
    TR.CurrentProb, pi/12, -pi/12, 0.0001);

% Perform the fuzzy logic reasoning
y_Linear = [Linear_SU_rule.Output, Linear_NC1_rule.Output, Linear_SD_rule.Output, Linear_NC2_rule.Output];
w_lower_Linear = [Linear_SU_rule.Interval(1), Linear_NC1_rule.Interval(1), ...
    Linear_SD_rule.Interval(1), Linear_NC2_rule.Interval(1)];
w_upper_Linear = [Linear_SU_rule.Interval(2), Linear_NC1_rule.Interval(2), ...
    Linear_SD_rule.Interval(2), Linear_NC2_rule.Interval(2)];

y_Angular = [Angular_TL_rule.Output, Angular_NC1_rule.Output, Angular_TR_rule.Output, Angular_NC2_rule.Output];
w_lower_Angular = [Angular_TL_rule.Interval(1), Angular_NC1_rule.Interval(1), ...
    Angular_TR_rule.Interval(1), Angular_NC2_rule.Interval(1)];
w_upper_Angular = [Angular_TL_rule.Interval(2), Angular_NC1_rule.Interval(2), ...
    Angular_TR_rule.Interval(2), Angular_NC2_rule.Interval(2)];

[yl_linear, yr_linear] = corrected_km(y_Linear, w_lower_Linear, w_upper_Linear);
[yl_Angular, yr_Angular] = corrected_km(y_Angular, w_lower_Angular, w_upper_Angular);
disp([yl_linear, yr_linear]);
disp([yl_Angular, yr_Angular]);



